package simpleGeom;


import java.awt.geom.Line2D;
import java.awt.geom.Point2D;

import processing.core.PVector;

public class Vec2D implements HasPos2D{
	public final static int CWRIGHT = 1;
	public final static int CCWLEFT = -1;
	public final static int PARALLEL = 0;
	public int id=0;
	public float x=0;
	public float y=0;
	
	public static void getRectByLine(float x1,float y1,float x2,float y2,float o1,float o2,float[] xCors,float[] yCors){
		float dX=x2-x1;
		float dY=y2-y1;
		float l=(float)Math.sqrt(dX*dX+dY*dY);
		float f1=o1/l;
		float f2=o2/l;
		xCors[0]=x1-f1*dY;
		xCors[1]=x2-f1*dY;
		xCors[2]=x2-f2*dY;
		xCors[3]=x1-f2*dY;
		
		yCors[0]=y1+f1*dX;
		yCors[1]=y2+f1*dX;
		yCors[2]=y2+f2*dX;
		yCors[3]=y1+f2*dX;
	}
	
	public static  HasPos2D getFromAtoBAbsolute(HasPos2D p1,HasPos2D p2,float d){
		float dX=p2.getX()-p1.getX();
		float dY=p2.getY()-p1.getY();
		float dist=(float)Math.sqrt(dX*dX+dY*dY);
		float f=d/dist;
		return new Vec2D(p1.getX()+f*dX,p1.getY()+f*dY);
	}
	
	public static HasPos2D getFromAtoBRelative(HasPos2D p1,HasPos2D p2,float f){
		float dX=p2.getX()-p1.getX();
		float dY=p2.getY()-p1.getY();
		return new Vec2D(p1.getX()+f*dX,p1.getY()+f*dY);
	}
	
	public static Polygon2D getRectByLine(float x1,float y1,float x2,float y2,float o1,float o2){
		float dX=x2-x1;
		float dY=y2-y1;
		float l=(float)Math.sqrt(dX*dX+dY*dY);
		float f1=o1/l;
		float f2=o2/l;
		Polygon2D poly=new Polygon2D();
		poly.addPt(x1-f1*dY, y1+f1*dX);
		poly.addPt(x2-f1*dY, y2+f1*dX);
		poly.addPt(x2-f2*dY, y2+f2*dX);
		poly.addPt(x1-f2*dY, y1+f2*dX);
		return poly;
	}
	
	public Vec2D(){
		this.x=0;
		this.y=0;
	}
	public Vec2D(float x,float y){
		this.x=x;
		this.y=y;
	}
	public Vec2D(double x,double y){
		this.x=(float)x;
		this.y=(float)y;
	}
	public Vec2D(HasPos2D v){
		this.x=v.getX();
		this.y=v.getY();
	}
	public float getX() {
		return x;
	}
	public void setX(float x) {
		this.x = x;
	}
	public float getY() {
		return y;
	}
	public void mult(float f){
		this.x*=f;
		this.y*=f;
	}
	public void div(float f){
		this.x/=f;
		this.y/=f;
	}
	public float mag(){
		return (float)Math.sqrt(magSqr());
	}
	public float magSqr(){
		return x*x+y*y;
	}
	
	public void setLength(float l){
		mult(l/this.mag());
	}
	public void normalize(){
		div(mag());
	}
	public void setY(float y) {
		this.y = y;
	}
	public void rot90(){
		float t=x;
		x=-y;
		y=t;
	}
	public void rot90m(){
		float t=x;
		x=y;
		y=-t;
	}
	public void add(HasPos2D v){
		this.x+=v.getX();
		this.y+=v.getY();
		
	}
	public void sub(HasPos2D v){
		this.x-=v.getX();
		this.y-=v.getY();
	}
	public float distSqr(float x,float y){
		return distSqr(this.x,this.y,x,y);
	}
	public float dist(float x,float y){
		return dist(this.x,this.y,x,y);
	}
	public float dist(HasPos2D p){
		return dist(x,y,p.getX(),p.getY());
	}
	public static Vec2D getPolar(double theta, double length) {
		double x =  length* Math.cos(theta);
		double y =  length* Math.sin(theta);
		
		return new Vec2D((float)x,(float) y);
}
	public static Vec2D getPolar(float oX,float oY,float theta, float length) {
		double x =  length* Math.cos(theta)+oX;
		double y =  length* Math.sin(theta)+oY;
		
		return new Vec2D((float)x,(float) y);
}
	public void setPosition(float x, float y) {
		// TODO Auto-generated method stub
		this.x=x;
		this.y=y;
	}
	public static Point2D sub(Point2D p1,Point2D p2){
		return new Point2D.Double(p1.getX()-p2.getX(),p1.getY()-p2.getY());
	}
	public static Vec2D sub(HasPos2D p1,HasPos2D p2){
		return new Vec2D(p1.getX()-p2.getX(),p1.getY()-p2.getY());
	}
	public static Vec2D add(HasPos2D p1,HasPos2D p2){
		return new Vec2D(p1.getX()+p2.getX(),p1.getY()+p2.getY());
	}
	public static Vec2D mult(HasPos2D p1,float m){
		return new Vec2D(p1.getX()*m,p1.getY()*m);
	}
	public static float parallelArea(double x0, double y0, double x1,
			double y1, double x2, double y2) {
		double d1X = x0 - x1;
		double d1Y = y0 - y1;
		double d2X = x0 - x2;
		double d2Y = y0 - y2;
		return (float)(d1X * d2Y - d2X * d1Y);
	}
	public static float parallelArea(HasPos2D p0,HasPos2D p1,HasPos2D p2) {
		return parallelArea(p0.getX(),p0.getY(),p1.getX(),p1.getY(),p2.getX(),p2.getY());
	}
	public static float distSqr(HasPos2D p0,HasPos2D p1) {
		float dX=p0.getX()-p1.getX();
		float dY=p0.getY()-p1.getY();
		return dX*dX+dY*dY;
	}
	public static float distSqr(HasPos2D p0,float x,float y) {
		float dX=p0.getX()-x;
		float dY=p0.getY()-y;
		return dX*dX+dY*dY;
	}
	public static float distSqr(double x1,double y1,double x2,double y2) {
		double dX=x1-x2;
		double dY=y1-y2;
		return (float)(dX*dX+dY*dY);
	}
	
	public static float dist(float x1,float y1,float x2,float y2) {
	
		return (float)Math.sqrt(distSqr(x1,y1,x2,y2));
	}
	public static float dist(HasPos2D p0,HasPos2D p1) {
		return (float)Math.sqrt(distSqr(p0,p1));
	}
	public static int relativeCCW(HasPos2D p0, HasPos2D p1, HasPos2D p2) {
		double ar = parallelArea(p0, p1, p2);
		if (Math.abs(ar) < 0.00000001)
			return PARALLEL;
		if (ar > 0)
			return CCWLEFT;
		return CWRIGHT;
	}
	public static boolean equalPosition(HasPos2D v1, HasPos2D v2, float tol){
		if (dist(v1, v2)<tol)
		return true;
		return false;
	}
	public static Vec2D rot90(HasPos2D v){
		return new Vec2D(-v.getY(),v.getX());
	}
	public static Vec2D rot90m(HasPos2D v){
		return new Vec2D(v.getY(),-v.getX());
	}
	@Override
	public String toString() {
		// TODO Auto-generated method stub
		return super.toString()+" x: "+x+" y: "+y;
	}
	public static Vec2D lineLineIntersection(HasPos2D[] l1,HasPos2D[] l2) {
		return lineLineIntersection(l1[0],l1[1],l2[0],l2[1]);
	}
	public static Vec2D lineLineIntersection(HasPos2D pA1, HasPos2D pA2,
			HasPos2D pB1, HasPos2D pB2) {
		Vec2D dir1 = sub(pA2, pA1);
		Vec2D dir2 = sub(pB2, pB1);
		return lineLineIntersectionVec(pA1, dir1, pB1, dir2);
	}
	
	public static Vec2D lineLineIntersection(Edge2D e1, Edge2D e2) {
		
		return lineLineIntersection(e1.getP1(),e1.getP2(),e2.getP1(),e2.getP2());
	}

	public static Vec2D lineLineIntersectionVec(HasPos2D pA1,
			HasPos2D dir1, HasPos2D pB1, HasPos2D dir2) {
		double a1 = dir1.getY();
		double b1 = -dir1.getX();
		double c1 = a1 * pA1.getX() + b1 * pA1.getY();
		// C = A*x1+B*y1
		double a2 = dir2.getY();
		double b2 = -dir2.getX();
		double c2 = a2 * pB1.getX() + b2 * pB1.getY();

		double det = a1 * b2 - a2 * b1;
		if (Math.abs(det) < 0.00000001) {
			return null;
			// Lines are parallel
		} else {
			double x = (b2 * c1 - b1 * c2) / det;
			double y = (a1 * c2 - a2 * c1) / det;
			return new Vec2D((float)x, (float)y);
		}
	}
	public static Vec2D segmentSegmentIntersection(Point2D a1,Point2D a2,Point2D b1,Point2D b2) {
		return segmentSegmentIntersection(a1.getX(),a1.getY(),a2.getX(),a2.getY(),b1.getX(),b1.getY(),b2.getX(),b2.getY());
	}
	public static Vec2D segmentSegmentIntersection(double aX, double aY,
			double bX, double bY, double cX, double cY, double dX, double dY) {

		double denominator = ((bX - aX) * (dY - cY)) - ((bY - aY) * (dX - cX));

		if (denominator == 0)
			return null;// parallel

		double numerator = ((aY - cY) * (dX - cX)) - (aX - cX) * (dY - cY);
		double numerator2 = ((aY - cY) * (bX - aX)) - ((aX - cX) * (bY - aY));

		double r = numerator / denominator;
		double s = numerator2 / denominator;

		if (s <= 0 || s >= 1 || r <= 0 || r >= 1)
			return null;// colinear

		double x = aX + r * (bX - aX);
		double y = aY + r * (bY - aY);
		return new Vec2D((float)x, (float)y);
	}
	public static Vec2D segmentLineIntersection(double aX, double aY,
			double bX, double bY, double cX, double cY, double dX, double dY) {

		double denominator = ((bX - aX) * (dY - cY)) - ((bY - aY) * (dX - cX));

		if (denominator == 0)
			return null;// parallel

		double numerator = ((aY - cY) * (dX - cX)) - (aX - cX) * (dY - cY);
		double numerator2 = ((aY - cY) * (bX - aX)) - ((aX - cX) * (bY - aY));

		double r = numerator / denominator;
		double s = numerator2 / denominator;

		if (s <= 0 || s >= 1 )
			return null;

		double x = aX + r * (bX - aX);
		double y = aY + r * (bY - aY);
		return new Vec2D((float)x, (float)y);
	}
	public static Vec2D lineLineIntersection(double aX, double aY,
			double bX, double bY, double cX, double cY, double dX, double dY) {
		double denominator = ((bX - aX) * (dY - cY)) - ((bY - aY) * (dX - cX));
		if (denominator == 0)
			return null;// parallel
		double numerator = ((aY - cY) * (dX - cX)) - (aX - cX) * (dY - cY);
		double numerator2 = ((aY - cY) * (bX - aX)) - ((aX - cX) * (bY - aY));
		double r = numerator / denominator;
		double s = numerator2 / denominator;
		double x = aX + r * (bX - aX);
		double y = aY + r * (bY - aY);
		return new Vec2D((float)x, (float)y);
	}
	public static Vec2D getOffsetPoint(HasPos2D p1, HasPos2D p2,
			HasPos2D p3, float offset1, float offset2) {
	
		if (!Line2D.linesIntersect(p1.getX(), p1.getY(), p2.getX(), p2.getY(),
				p2.getX(), p2.getY(), p3.getX(), p3.getY())) {
			return null;
		}
	
		else {
			Line2D.Double oL1 = getOffsetLine(p1, p2, offset1);
			Line2D.Double oL2 = getOffsetLine(p2, p3, offset2);
			return lineLineIntersection(oL1.x1,oL1.y1,oL1.x2,oL1.y2, oL2.x1,oL2.y1,oL2.x2,oL2.y2);
		}
	}
	
	public static Vec2D getOrthoOffsetPoint(HasPos2D p1, HasPos2D p2,
			float offset){
		Vec2D v=sub(p2, p1);
		v.setLength(offset);
		v.rot90();
		return add(p1, v);
	}
	public static Vec2D getOrthoOffsetPoint(float x1,float y1,float x2,float y2,
			float offset){
		Vec2D v1=new Vec2D(x1,y1);
		Vec2D v2=new Vec2D(x2,y2);
		return getOrthoOffsetPoint(v1,v2,offset);
	}
	public static Line2D.Double getOffsetLine(HasPos2D p1, HasPos2D p2,
			float offset) {
		Vec2D v=sub(p2, p1);
		v.setLength(offset);
		v.rot90();
		Vec2D p1Offset = add(p1, v);
		Vec2D p2Offset = add(p2, v);
		return new Line2D.Double(p1Offset.getX(),p1Offset.getY(), p2Offset.getX(),p2Offset.getY());
	}
	public static Edge2D getOffsetEdge(HasPos2D p1, HasPos2D p2,HasPos2D p3,HasPos2D p4,
			float offset) {
		return getOffsetEdge(p1,p2,p3,p4,offset,offset,offset);
		
	}
	public static Edge2D getOffsetEdge(HasPos2D p1, HasPos2D p2,HasPos2D p3,HasPos2D p4,
			float offset1,float offset2,float offset3) {
		
		Edge2D e=getOffsetEdge(p1,p2,offset2);
		Vec2D p1Offset = getOffsetPoint(p1,p2,p3,offset1,offset2);
		if (p1Offset==null)p1Offset=new Vec2D(e.p1);
		Vec2D p2Offset =getOffsetPoint(p2,p3,p4,offset2,offset3);
		if (p2Offset==null)p2Offset=new Vec2D(e.p2);
		return new Edge2D(p1Offset, p2Offset);
	}
	public static Edge2D getOffsetEdge(HasPos2D p1, HasPos2D p2,
			float offset) {
		Vec2D v=sub(p2, p1);
		v.setLength(offset);
		v.rot90();
		Vec2D p1Offset = add(p1, v);
		Vec2D p2Offset = add(p2, v);
		return new Edge2D(p1Offset, p2Offset);
	}
	public  static float dot(HasPos2D a,HasPos2D b) {
		return a.getX() * b.getX() + a.getY() * b.getY();
	}

	public static boolean isParallel(double a1x, double a1y,
			double a2x, double a2y, double b1x, double b1y, double b2x, double b2y) {
		double ax=a2x-a1x;
		double ay=a2y-a1y;
		double bx=b2x-b1x;
		double by=b2y-b1y;
		double det = ay * -bx - by * -ax;
		if (Math.abs(det) <= 0.00000001) {
			return true;
			// Lines are parallel
		} return false;
	}
	
	public static boolean isParallel(HasPos2D  a1,HasPos2D a2,HasPos2D  b1,HasPos2D b2) {	
		return isParallel(a1.getX(),a1.getY(),a2.getX(),a2.getY(),b1.getX(),b1.getY(),b2.getX(),b2.getY());
	}
	
	public static float getAngleBetween(Vec2D a, Vec2D b) {
		float a1=a.getAngle();
		float a2=b.getAngle();
		float dA=a2-a1;
		if (dA>Math.PI){
			//return dA;
		}
		Vec2D v1=new Vec2D(a);
		Vec2D v2=new Vec2D(b);
		v1.normalize();
		v2.normalize();
		double dot = dot(v1, v2);
		return (float)Math.acos(dot);
	}
	public   static double getTheta(Point2D p) {
		return  (double)Math.atan2(p.getY(),p.getX());
	}
	public   static float getTheta(HasPos2D p) {
		return  (float)Math.atan2(p.getY(),p.getX());
	}
	public  float getTheta() {
		return  getAngle();
	}
	public  float getAngle() {
		return  (float)Math.atan2(y, x);
	}
	public  static float getAngle(HasPos2D v) {
		return  (float)Math.atan2(v.getY(), v.getX());
	}
	public  static float getAngle(float x,float y) {
		return  (float)Math.atan2(y,x);
	}
	public  void rot(float angle) {
		float cAngle=getAngle();
		float cL=this.mag();
		Vec2D c=getPolar(cAngle+angle,cL);
	this.setPosition(c.getX(),c.getY());
	}
	public static Vec2D rotate(float angle, float origX, float origY, double x, double y){
		 x=x-origX;
		 y=y-origY;
		x=x*Math.cos(angle) - y* Math.sin(angle);
		y= y*Math.cos(angle) + x* Math.sin(angle);
		x=x+origX;
		y=y+origY;
		return new Vec2D(x,y);
	}
	float getAngle(HasLine2D l){
		float dX=l.getX2()-l.getX1();
		float dY=l.getY2()-l.getY1();
		return (float)Math.atan2(dY, dX);
	}
	public static float getAngleBetween(HasLine2D l1,HasLine2D l2){
		float dX1=l1.getX2()-l1.getX1();
		float dY1=l1.getY2()-l1.getY1();
		float dX2=l2.getX2()-l2.getX1();
		float dY2=l2.getY2()-l2.getY1();
		//return (float)Math.atan2(dY, dX);
		return getAngleBetween(new Vec2D(dX1,dY1),new Vec2D(dX2,dY2));
	}
	public static float getAngleBetween(HasPos2D p1,HasPos2D p2,HasPos2D p3){
		Vec2D v1=Vec2D.sub(p1,p2);
		Vec2D v2=Vec2D.sub(p3,p2);
		//return (float)Math.atan2(dY, dX);
		return getAngleBetween(v1,v2);
	}
	
	public static Vec2D getClosestPointToSeg(HasPos2D p, HasPos2D pa,
			HasPos2D pb) {
		Vec2D vab = Vec2D.sub(pb,pa);
		float t = ((p.getX() - pa.getX()) * vab.getX() + (p.getY() - pa.getY())
				* vab.getY())
				/ vab.distSqr(0, 0);
		if (t <= 0)
			return new Vec2D(pa);
		else if (t >= 1)
			return new Vec2D(pb);
		return new Vec2D(pa.getX() + (t * vab.getX()), pa.getY()
				+ (t * vab.getY()));
	}
	
	public static float[] getClosestPointToSeg(float x1,float y1,float x2,float y2,float pX,float pY,float[]result) {
		if (result==null)result=new float[2];
		float dX=x2-x1;
		float dY=y2-y1;
		float t = ((pX-x1) * dX + (pY-y1)* dY)/ (dX*dX+dY*dY);
		if (t <= 0){
			result[0]=x1;
			result[1]=y1;
		}
		else if (t >= 1){
			result[0]=x2;
			result[1]=y2;
		}
		else{
			result[0]=x1+t*dX;
			result[1]=y1+t*dY;
		}
		return result;
	}
	
	public static float getDistToSegSq(float x1,float y1,float x2,float y2,float pX,float pY) {
		float dX=x2-x1;
		float dY=y2-y1;
		float vX,vY=0;
		float t = ((pX-x1) * dX + (pY-y1)* dY)/ (dX*dX+dY*dY);
		if (t <= 0){
			vX=x1-pX;
			vY=y1-pY;
		}
		else if (t >= 1){
			vX=x2-pX;
			vY=y2-pY;
		}
		else{
			vX=x1+t*dX-pX;
			vY=y1+t*dY-pY;
		}
		return vX*vX+vY*vY;
	}
	public static float getDistToSeg(float x1,float y1,float x2,float y2,float pX,float pY) {
		return (float)Math.sqrt(getDistToSegSq(x1,y1,x2,y2,pX,pY));
	}
	public static double getAngleBetweenCCW(HasPos2D p, HasPos2D a, HasPos2D b) {
		Vec2D va = Vec2D.sub(p, a);
		Vec2D vb = Vec2D.sub(p, b);
		return getAngleBetweenCCW(Math.atan2(vb.y, vb.x),Math.atan2(va.y, va.x));
		
	}
	public static double getAngleBetweenCCW(double theta1, double theta2) {
		double cAngle = theta1 - theta2;
		if (cAngle < 0) {
			return cAngle + 2.0 * Math.PI;
		}
		return cAngle;
	}
	/*public static HasPos2D getClosestPointToSeg(HasPos2D p, Line2D line) {
		return getClosestPointToSeg(p, line.getP1(), line.getP2());
	}*/
	public int getId() {
		// TODO Auto-generated method stub
		return id;
	}
	public void setId(int i) {
		// TODO Auto-generated method stub
		this.id=i;
	}
}
